%% GRNN Function Approximation
% Modified demo demogrn1.m to learn the simulated arm data.
% This demonstration uses functions NEWGRNN and SIM.
%

N = size(samples,1)
Ntrain = floor(.9*N)

for spread = 0.3:.05:0.3
    
    disp(['Spread = ' num2str(spread)]);
    
    %%
    % Here are eight data points of a function we would like to fit.  The functions
    % inputs P should result in target outputs T.
    
    P = [samples(1:Ntrain-1,1:2:3) samples(2:Ntrain,1:2:3)]';
    T = samples(2:Ntrain,5)'; % the torque command is stored with the target it reached (next sample).
    
    figure(1)
    plot3(samples(:,1),samples(:,3),samples(:,5));xlabel('theta'),ylabel('dtheta');zlabel('tau')
    
    %%
    % We use NEWGRNN to create a generalized regression network.  We use a SPREAD
    % slightly lower than 1, the distance between input values, in order, to get a
    % function that fits individual data points fairly closely.  A smaller spread
    % would fit data better but be less smooth.
    
    net = newgrnn(P,T,spread);
    A = sim(net,P);
    
    
    figure(2)
    plot([A',T']);
    title('original data');
    mean(abs(A-T),2)
    
    %%
    % Test on new data not used during training.
    
    P = [samples(Ntrain+1:end-1,1:2:3) samples(Ntrain+1+1:end,1:2:3)]';
    T = samples(Ntrain+1+1:end,5)';
    
    A = sim(net,P);
    
    figure(3)
    plot([A',T']);
    title('novel data');
    mean(abs(A-T),2)
    
    
end



%%
% TEST IT on the arm to generate starting and stopping commands.
% We can use the network to approximate the function at a new input value.

% see what the simulator will see.
addpath('robot');

mytwolink;  % define a 2D monkey arm with dynamixel motors!

n=1;
j=1;
m=1;
for theta_t0 = 0:pi/4:pi;
    k=1;
    for theta_target = (theta_t0- pi/4):pi/16:(theta_t0+pi/4)
        if(theta_target < 0 || theta_target > pi)
            k=k+1;
            continue;
        end
        
        disp('*************');
        disp(['theta: ' num2str(theta_t0) ' theta_target: ' num2str(theta_target)]);
        
        thetad_t0 = 0;
        thetad_target = 0;
        
        max_thetad = 7*(theta_target-theta_t0); % rad/sec; this was about the mean absolute value of the velocity in the babbling data;
        
        p = [theta_t0; thetad_t0; (theta_t0+theta_target)/2; max_thetad]; % ag1
        ag1 = sim(net,p);
        disp('intermediate target: ');
        disp(p);
        
        
        
        p = [ (theta_t0+theta_target)/2 ;  max_thetad; theta_target ; thetad_target]; % ag1
        ant1 = sim(net,p);
        
        % DEBUG test on a known answer.
        % theta_t0 = samples(2000-37,1);
        % theta_target = samples(2000-35,1);
        %
        % thetad_t0 = 2.5825;
        % thetad_target = 2.3848;
        % max_thetad = -.1727; % rad/sec;
        % ag1=samples(2000-36,5);
        % ant1= samples(2000-35,5);
        
        
        disp(['ag1: ' num2str(ag1), ' ant1: ', num2str(ant1)]);
        
        % This is my estimated command, now see where it ends up.
        
        % ag1
        
        t=0;
        q= [theta_t0  0 ];
        qd=[thetad_t0 0 ];
        tau = [ag1 0];
        
        
        % agonist simulation.
        for i = 1:10
            [t2 q2 qd2 qdd] = myfdyn(tl, t,t+.01, tau,q,qd, [0 0], [pi pi]);
            t=t2(2);q=q2(2,:);qd=qd2(2,:);
            %plot(tl,q);
            data(n,:) = [q(1) qd(1)];
            n=n+1;
        end
        
        q(1,:)
        qd(1,:)
        
        perror(j,k) =  (theta_t0+theta_target)/2 -  q(1,1);
        pderror(j,k)= max_thetad- qd(1,1);

        targets(m)=(theta_t0+theta_target)/2;
        targetx(m)=n;
        m=m+1;
        
        
        tau = [ant1 0]; % assumes that ant1 commands happens without feedback.  With feedback I could recalculate ant1 after ag1 result, allowing online correction.
        %tau = [-ag1 0];
        
        % 100 msec simulation.
        for i = 1:10
            [t2 q2 qd2 qdd] = myfdyn(tl, t,t+.01, tau,q,qd, [0 0], [pi pi]);
            t=t2(2);q=q2(2,:);qd=qd2(2,:);
            %plot(tl,q);
            data(n,:) = [q(1) qd(1)];
            n=n+1;
        end
        
        targets(m)=theta_target;
        targetx(m)=n;
        m=m+1;
        %perror(j,k) =  theta_target-  q(1,1);
        %pderror(j,k)= thetad_target- qd(1,1);
        
        k=k+1;
    end
    j=j+1;
end
%%
%
n

figure(4)
plot((-pi/4:pi/16:pi/4)*180/pi,.5*(-pi/4:pi/16:pi/4)*180/pi)
hold on
plot((-pi/4:pi/16:pi/4)*180/pi,mean(perror)*180/pi,'r.')


figure(5)
plot(data,'.');legend('theta','thetadot');
hold on;
plot(targetx,targets,'mo');
hold off;

%% 
% Now train a network to learn the stopping command.
% Here, we only want to stop the arm, not slow down to arbitrary speeds?
%
% I could take p(t), pd(t), tau, and p(t+100), and pd(t+100)=0, to generate stopping
% command and generate it after with a 100 ms. delay.

% Use supervised learning to find tau that gives p(t+100).  This is CB
% like.










